#include "ActionController.h"
#define MAX_BALLOON 30

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange);
int actionSee(IplImage *dest, Balloon *targets);
bool thereAreTarget(int num_target);
void getRange(ArRangeDevice *mySonar,double *centralRange,double *leftRange,double *rightRange,double radius);

ActionController::ActionController(double stopDistance, double controlAngle,
		double velocityOnTarget, double velocityNearHurdle, double maxVelocity, ActionGo *go,ActionTurn *turn,ActionScouting *scout) :
	ArAction("Controller") {

	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	myVelocityOnTarget = velocityOnTarget;
	myVelocityNearHurdle=velocityNearHurdle;
	myMaxVelocity = maxVelocity;

	myGo = go;
	myTurn = turn;
	myScout=scout;

	connectToCam(1);
}


void ActionController::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

ArActionDesired *ActionController::fire(ArActionDesired currentDesired) {

	/* Dichiarazione delle variabili  Visione*/
	IplImage *src;
	int num_target=0;
	int num_obstacle=0;
	Balloon targets[MAX_BALLOON];
	Balloon obstacles[MAX_BALLOON];

	/* Dichiarazione delle variabili  Robot*/
	double centralRange,leftRange,rightRange;
	double radius =myRobot->getRobotRadius();
	bool target=false;
	double distLeft=200,distRigh=200;
	double degree=0,degreeRange=60;
	bool isY=false;
	bool isO=false;
	double range;

	/* Acquisizione delle grandezze di interesse */
	myDesired.reset();

	/* Faccio un istantanea della scena */
	src = connectToCam(0);
	num_target=getTargets(src, targets,cvPoint(src->width/2, src->height));
	num_obstacle=getObstacles(src, obstacles,cvPoint(src->width/2, src->height));


	/* Leggo i valori dei Sonar */
	range = mySonar->currentReadingPolar(-myControlAngle, myControlAngle)- myRobot->getRobotRadius();
	getRange(mySonar,&centralRange,&leftRange,&rightRange,radius);


	target=thereAreTarget(num_target);

	int near_t = 0;
	int near_o = 0;
	bool nYel = true;

	//imposto i degreeRange in base alla distanza per il Raggiungi target
	if(range>600)
		degreeRange=50;
	else
		degreeRange=70;

	if(num_target>0)
	     near_t = nearest(targets, num_target);

	//se ho un ostacolo e un target nella visuale controllo se è più vicino il giallo o l'arancio
	if(num_obstacle>0 && num_target>0){
		near_o = nearest(obstacles, num_obstacle);

		Balloon theNearest[2] = {targets[near_t], obstacles[near_o]};

		int near_t_o = nearest(theNearest, 2);

		if (near_t_o==0){

			nYel=true;
		}
		else{

			nYel=false;
		}

	}

	/*Individuazione stato in cui andare
	 * State=1 scoppia pallone
	 * State=2 evita ostacolo
	 * State=3 raggiungi taget
	 * State=4 esplora
	 * Quando vado nello stato 2, dobbiamo controllare i sensori in modo opportuno
	 * per capire se devo aggirare o meno un ostacolo per raggiungere il pallone giallo
	 * */
	//controllo se c'è un giallo e un arancio d'avanti
	isY=isYellow(src,cvPoint(src->width/2, src->height),144,128,40);
	isO=isOrange(src,cvPoint(src->width/2, src->height),144,128,40);

	if(range<myStopDistance  || isY || isO){

		if(isY && !isO && ((leftRange > distLeft && rightRange>distRigh)||nYel))
			state=SCOPPIA_PALLONE;
		else{
			if(myScout->isActive() && (myScout->left==false || myScout->right==false))
				state=ESPLORA;
			else state=EVITA_OSTACOLO;
		}

	}else {
			//se ho un obbiettivo lo raggiungo
			if(target)
				state=RAGGIUNGI_TARGET;
				//altrimenti vado in esplorazione
				else
					state=ESPLORA;
	}

	/*Esecuzione delle azioni opportune*/

	switch (state){
		case SCOPPIA_PALLONE: printf("State1: Scoppio Pallone \n");
								//disattivo azioni
								if(myTurn->isActive())
									myTurn->deactivate();
								if(myGo->isActive())
									myGo->deactivate();
								if(myScout->isActive())
									myScout->deactivate();

								myDesired.setVel(myVelocityOnTarget);
								break;
		case EVITA_OSTACOLO: printf("State2: Evita Ostacolo \n");

								if(myScout->isActive())
									myScout->deactivate();


								if(!myTurn->isActive())
									 myTurn->activate();

								if(myTurn->myTurning==0 && isO)
									myDesired.setRotVel(myScout->sign*40);

								if(myRobot->getVel()==0)
									myDesired.setVel(myVelocityNearHurdle);
								if(num_target>0){
									// la turn dice che se -1 l'ostacolo è a sx e noi giriamo a dx
									// altrimenti 1
								   if(myTurn->myTurning<0)
										myScout->sign=-1;
									else myScout->sign=1;

								    myScout->angleLeft=90;
								    myScout->angleRight=390;
								}else {
									printf("turning %d\n\n",myTurn->myTurning);
									  if(myTurn->myTurning<0){
										    myScout->sign=1;
										    myScout->angleLeft=0;
										    myScout->angleRight=390;
									  }else {
											myScout->sign=-1;
										    myScout->angleLeft=390;
										    myScout->angleRight=0;

									  }

								}

								break;
		case RAGGIUNGI_TARGET: printf("State3: Raggiungi Target \n");

								degree = targets[near_t].angle;


								if(myTurn->isActive())
									myTurn->deactivate();

								if(myScout->isActive())
									myScout->deactivate();

								if(!myGo->isActive())
									myGo->activate();

								analizeDegree(&myDesired,degree,degreeRange);

								break;
		case ESPLORA: printf("State4: Esplora \n");

						if(myTurn->isActive())
							myTurn->deactivate();

						if (myGo->isActive())
							myGo->deactivate();

						if (!myScout->isActive()){
							myScout->startAngle=myRobot->getTh();
							myScout->angle=0;
							myScout->left=true;
							myScout->right=true;
							myScout->activate();
						}

						break;
	}

	return &myDesired;
}

/* Questa azione acquisice un frame dalla scena e restituisce l'immagine e il numero di
 * target presenti
 * */
bool thereAreTarget(int num_target){
	if(num_target>0)
		return true;
	else return false;
}
void getRange(ArRangeDevice *mySonar,double *centralRange,double *leftRange,double *rightRange,double radius){
	double centralAngle =10;
	double leftAngle1 =30,leftAngle2 =90;
	*centralRange=mySonar->currentReadingPolar(-centralAngle, centralAngle)- radius;
	*leftRange=mySonar->currentReadingPolar(leftAngle1, leftAngle2)- radius;
	*rightRange=mySonar->currentReadingPolar(-leftAngle2, -leftAngle1)- radius;

}
void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange){

	if (degree > degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(-10);
	} else if (degree < -degreeRange) {
				myDesired->setVel(0);
				myDesired->setDeltaHeading(10);
			} else
				myDesired->setDeltaHeading(0);

}
